/*=====================================================================*
 *                   Copyright (C) 2011 Paul Mineiro                   *
 * All rights reserved.                                                *
 *                                                                     *
 * Redistribution and use in source and binary forms, with             *
 * or without modification, are permitted provided that the            *
 * following conditions are met:                                       *
 *                                                                     *
 *     * Redistributions of source code must retain the                *
 *     above copyright notice, this list of conditions and             *
 *     the following disclaimer.                                       *
 *                                                                     *
 *     * Redistributions in binary form must reproduce the             *
 *     above copyright notice, this list of conditions and             *
 *     the following disclaimer in the documentation and/or            *
 *     other materials provided with the distribution.                 *
 *                                                                     *
 *     * Neither the name of Paul Mineiro nor the names                *
 *     of other contributors may be used to endorse or promote         *
 *     products derived from this software without specific            *
 *     prior written permission.                                       *
 *                                                                     *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND              *
 * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,         *
 * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES               *
 * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE             *
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER               *
 * OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,                 *
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES            *
 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE           *
 * GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR                *
 * BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF          *
 * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT           *
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY              *
 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE             *
 * POSSIBILITY OF SUCH DAMAGE.                                         *
 *                                                                     *
 * Contact: Paul Mineiro <paul@mineiro.com>                            *
 *=====================================================================*/

#ifndef __NORMALIZE_H__
#define __NORMALIZE_H__

#include "fastonebigheader.h"

#ifdef __SSE2__
static float
safe_exponentiate_normalize (float*       x,
                             unsigned int n)
{
  /*
   * ok we want to compute 
   * 
   *            exp (x[i])          exp (x[i] - max)
   * x[i] = ----------------- = ------------------------
   *        sum_i exp (x[i])     sum_i exp (x[i] - max)
   *
   * the RHS is more numerically stable
   */

  float max = x[0];
  v4sf vmax = _mm_load1_ps (&x[0]);
  unsigned int i;

  for (i = 0; i < n && (((uintptr_t) x) % 16) > 0; ++i)
    {
      max = (max < x[i]) ? x[i] : max;
    }

  for (; i + 4 < n; i += 4)
    {
      v4sf arg = _mm_load_ps (x + i);
      vmax = _mm_max_ps (vmax, arg);
    }

  for (; i < n; ++i)
    {
      max = (max < x[i]) ? x[i] : max;
    }

  max = (max < v4sf_index (vmax, 0)) ? v4sf_index (vmax, 0) : max;
  max = (max < v4sf_index (vmax, 1)) ? v4sf_index (vmax, 1) : max;
  max = (max < v4sf_index (vmax, 2)) ? v4sf_index (vmax, 2) : max;
  max = (max < v4sf_index (vmax, 3)) ? v4sf_index (vmax, 3) : max;
  vmax = _mm_load1_ps (&max);

  float denominator = 0;
  v4sf vdenominator = _mm_setzero_ps ();

  for (i = 0; i < n && (((uintptr_t) x) % 16) > 0; ++i)
    {
      x[i] = fastexp (x[i] - max);
      denominator += x[i];
    }

  for (; i + 4 < n; i += 4)
    {
      v4sf varg = _mm_load_ps (x + i);
      varg = vfastexp (varg - vmax);
      _mm_store_ps (x + i, varg);
      vdenominator += varg;
    }

  for (; i < n; ++i)
    {
      x[i] = fastexp (x[i] - max);
      denominator += x[i];
    }

  denominator += v4sf_index (vdenominator, 0) +
                 v4sf_index (vdenominator, 1) +
                 v4sf_index (vdenominator, 2) +
                 v4sf_index (vdenominator, 3);

  vdenominator = _mm_load1_ps (&denominator);

  float q = 0;
  v4sf vq = _mm_setzero_ps ();

  for (i = 0; i < n && (((uintptr_t) x) % 16) > 0; ++i)
    {
      x[i] /= denominator;
      q -= (x[i] > 0) ? x[i] * fastlog (x[i]) : 0;
    }

  for (; i + 4 < n; i += 4)
    {
      v4sf arg = _mm_load_ps (x + i) / vdenominator;
      _mm_store_ps (x + i, arg);
      v4sf gtzero = _mm_cmpgt_ps (arg, _mm_setzero_ps ());
      v4sf logarg = vfastlog (arg);
      vq -= _mm_and_ps (gtzero, arg * logarg);
    }

  for (; i < n; ++i)
    {
      x[i] /= denominator;
      q -= (x[i] > 0) ? x[i] * fastlog (x[i]) : 0;
    }

  q += v4sf_index (vq, 0) +
       v4sf_index (vq, 1) +
       v4sf_index (vq, 2) +
       v4sf_index (vq, 3);

  return q;
}
#else
#warning "not using SSE2"
static float
safe_exponentiate_normalize (float*       x,
                             unsigned int n)
{
  /*
   * ok we want to compute 
   * 
   *            exp (x[i])          exp (x[i] - max)
   * x[i] = ----------------- = ------------------------
   *        sum_i exp (x[i])     sum_i exp (x[i] - max)
   *
   * the RHS is more numerically stable
   */

  unsigned int maxindex = 0;

  for (unsigned int i = 1; i < n; ++i)
    {
      if (x[i] > x[maxindex])
        {
          maxindex = i;
        }
    }

  float denominator = 0;
  float max = x[maxindex];

  for (unsigned int i = 0; i < n; ++i)
    {
      x[i] = fastexp (x[i] - max);
      denominator += x[i];
    }

  float q = 0;
  for (unsigned int i = 0; i < n; ++i)
    {
      x[i] /= denominator;
      q -= (x[i] > 0) ? x[i] * fastlog (x[i]) : 0;
    }

  return q;
}
#endif

#endif /* __NORMALIZE_H__ */
